#include "AP_ComplFilter.h"

AP_ComplFilter::AP_ComplFilter(const AP_AHRS *ahrs, const RangeFinder &rng) :
    _ahrs(ahrs),
    _rng(rng)
{
    // Z axis time constant
    if( is_zero(AP_INTERTIALNAV_TC_Z)) {
        _k1_z = _k2_z = _k3_z = 0.0f;
    }else{
        _k1_z = 3 / AP_INTERTIALNAV_TC_Z;
        _k2_z = 3 / (AP_INTERTIALNAV_TC_Z*AP_INTERTIALNAV_TC_Z);
        _k3_z = 1 / (AP_INTERTIALNAV_TC_Z*AP_INTERTIALNAV_TC_Z*AP_INTERTIALNAV_TC_Z);
    }
}

// update - updates velocities and positions using latest info from ahrs and rngfnd if new data is available;
void AP_ComplFilter::update(float dt)
{
    // discard samples where dt is too large
    if( dt > 0.1f ) {
        return;
    }

    // check if new rngfnd readings have arrived and use them to correct vertical accelerometer offsets.
    check_rngfnd();

    float accel_ef_z = _ahrs->get_accel_ef().z;

    // remove influence of gravity
    accel_ef_z += GRAVITY_MSS;
    accel_ef_z *= 100;

    accel_correction_ef += _position_error * _k3_z  * dt;
    _velocity += _position_error * _k2_z  * dt;
    _position_correction += _position_error * _k1_z  * dt;

    // calculate velocity increase adding new acceleration from accelerometers
    const float &velocity_increase = (accel_ef_z + accel_correction_ef) * dt;

    // calculate new estimate of position
    _position_base += (_velocity + velocity_increase*0.5) * dt;

    // update the corrected position estimate
    _position = _position_base + _position_correction;

    // calculate new velocity
    _velocity += velocity_increase;

    _velocity = constrain_float(_velocity, -70.0f, 70.0f);

    // store 3rd order estimate (i.e. estimated vertical position) for future use
    _hist_position_estimate_z.push_back(_position_base);
}

void AP_ComplFilter::check_rngfnd()
{
    bool rngfnd_healthy = ((_rng.status_orient(ROTATION_PITCH_270) == RangeFinder::RangeFinder_Good) 
        && (_rng.range_valid_count_orient(ROTATION_PITCH_270) >= 3));

    if (rngfnd_healthy) {
        float rngfnd_cm;
        const float tilt_correction = MAX(0.707f, _ahrs->get_rotation_body_to_ned().c.z);
        rngfnd_cm = _rng.distance_cm_orient(ROTATION_PITCH_270);
        rngfnd_cm *= tilt_correction;
        correct_with_rngfnd(rngfnd_cm);
    } else {
        _position_base = 0;
        _position_correction = 0;
        _position = 0;
    }
}

// correct_with_rngfnd - modifies accelerometer offsets using rngfnd.
void AP_ComplFilter::correct_with_rngfnd(float rngfnd)
{
    static uint8_t first_reads = 0;

    // discard first 7 reads but perform some initialisation
    if( first_reads <= 7 ) {
        set_altitude(rngfnd);
        first_reads++;
    }

    // 3rd order samples (i.e. position from rngfnd) are delayed by 50ms (20 iterations at 400hz)
    // so we should calculate error using historical estimates
    float hist_position_base_z;
    if( _hist_position_estimate_z.is_full() ) {
        hist_position_base_z = _hist_position_estimate_z.front();
    }else{
        hist_position_base_z = _position_base;
    }

    // calculate error in position from rngfnd with our estimate
    _position_error = rngfnd - (hist_position_base_z + _position_correction);
}

// set_altitude - set base altitude estimate in cm
void AP_ComplFilter::set_altitude( float new_altitude)
{
    _position_base = new_altitude;
    _position_correction = 0;
    _position = new_altitude; // _position = _position_base + _position_correction
}